#include <stdio.h>
#include <math.h>

/**
 * 模糊PID算法实现
 * 采用模糊PID算法，结合模糊控制理论
 * 适用于非线性系统和模糊控制
 */

// 模糊集合定义
typedef enum
{
    NB, // 负大
    NS, // 负小
    ZO, // 零
    PS, // 正小
    PB  // 正大
} FuzzySet;

// 模糊PID控制器结构体
typedef struct
{
    // 基础PID参数
    float Kp, Ki, Kd;
    float integral;
    float prev_error;
    float prev_measure;

    // 模糊控制参数
    float e_scale;       // 误差量化因子
    float de_scale;      // 误差变化率量化因子
    float deltaKp_scale; // Kp调整量化因子
    float deltaKi_scale; // Ki调整量化因子
    float deltaKd_scale; // Kd调整量化因子

    // 隶属度函数参数（三角形）
    float e_params[5][3]; // [NB, NS, ZO, PS, PB]的{a, b, c}
    float de_params[5][3];
} FuzzyPIDController;

// 初始化模糊PID控制器
void FuzzyPID_Init(FuzzyPIDController *fpid)
{
    // 初始化PID参数
    fpid->integral = 0.0f;
    fpid->prev_error = 0.0f;
    fpid->prev_measure = 0.0f;

    // 量化因子初始化（根据实际系统调整）
    fpid->e_scale = 0.2f;  // 误差范围[-5,5]
    fpid->de_scale = 0.5f; // 误差变化率范围[-2,2]
    fpid->deltaKp_scale = 0.3f;
    fpid->deltaKi_scale = 0.1f;
    fpid->deltaKd_scale = 0.05f;

    // 误差隶属度函数参数（三角形左底点a, 顶点b, 右底点c）
    float e_params[5][3] = {
        {-7.0f, -5.0f, -3.0f}, // NB
        {-5.0f, -3.0f, -1.0f}, // NS
        {-2.0f, 0.0f, 2.0f},   // ZO
        {1.0f, 3.0f, 5.0f},    // PS
        {3.0f, 5.0f, 7.0f}     // PB
    };
    memcpy(fpid->e_params, e_params, sizeof(e_params));

    // 误差变化率隶属度函数
    float de_params[5][3] = {
        {-3.0f, -2.0f, -1.0f}, // NB
        {-2.0f, -1.0f, 0.0f},  // NS
        {-1.0f, 0.0f, 1.0f},   // ZO
        {0.0f, 1.0f, 2.0f},    // PS
        {1.0f, 2.0f, 3.0f}     // PB
    };
    memcpy(fpid->de_params, de_params, sizeof(de_params));
}

// 三角形隶属度计算
float triangle_mf(float x, float a, float b, float c)
{
    if (x <= a || x >= c)
        return 0.0f;
    return (x < b) ? (x - a) / (b - a) : (c - x) / (c - b);
}

// 模糊推理核心
void fuzzy_inference(FuzzyPIDController *fpid, float e, float de,
                     float *deltaKp, float *deltaKi, float *deltaKd)
{
    // 1. 模糊化：计算输入变量的隶属度
    float e_degree[5] = {0};
    float de_degree[5] = {0};

    // 量化输入
    float q_e = e * fpid->e_scale;
    float q_de = de * fpid->de_scale;

    // 计算误差隶属度
    for (int i = 0; i < 5; i++)
    {
        e_degree[i] = triangle_mf(q_e,
                                  fpid->e_params[i][0],
                                  fpid->e_params[i][1],
                                  fpid->e_params[i][2]);
    }

    // 计算误差变化率隶属度
    for (int i = 0; i < 5; i++)
    {
        de_degree[i] = triangle_mf(q_de,
                                   fpid->de_params[i][0],
                                   fpid->de_params[i][1],
                                   fpid->de_params[i][2]);
    }

    // 2. 规则评估（使用简化规则表）
    // 规则格式：IF e IS A AND de IS B THEN deltaKp IS C, deltaKi IS D, deltaKd IS E
    // 这里使用典型模糊PID规则库（可根据实际系统修改）
    static const struct
    {
        FuzzySet e, de;
        float dKp, dKi, dKd;
    } rules[] = {
        // 误差较大时，快速响应
        {NB, NB, 0.8f, 0.0f, 0.1f},
        {NB, NS, 0.6f, 0.0f, 0.2f},
        {PB, PB, 0.8f, 0.0f, 0.1f},
        // 误差中等时，抑制超调
        {NS, ZO, 0.3f, 0.1f, 0.3f},
        {PS, ZO, 0.3f, 0.1f, 0.3f},
        // 误差较小时，精细调节
        {ZO, ZO, 0.1f, 0.2f, 0.5f},
        {ZO, PS, 0.0f, 0.3f, 0.6f},
        {ZO, NS, 0.0f, 0.3f, 0.6f}};

    // 3. 解模糊（加权平均法）
    float total_weight = 0.0f;
    *deltaKp = *deltaKi = *deltaKd = 0.0f;

    for (int i = 0; i < sizeof(rules) / sizeof(rules[0]); i++)
    {
        // 计算规则激活强度（取最小值）
        float weight = fminf(
            e_degree[rules[i].e],
            de_degree[rules[i].de]);

        if (weight > 0)
        {
            total_weight += weight;
            *deltaKp += weight * rules[i].dKp;
            *deltaKi += weight * rules[i].dKi;
            *deltaKd += weight * rules[i].dKd;
        }
    }

    if (total_weight > 0)
    {
        *deltaKp /= total_weight;
        *deltaKi /= total_weight;
        *deltaKd /= total_weight;
    }
}

// 模糊PID计算
float FuzzyPID_Compute(FuzzyPIDController *fpid, float setpoint, float measure, float dt)
{
    // 计算误差项
    float error = setpoint - measure;
    float de = (error - fpid->prev_error) / dt;

    // 模糊推理调整参数
    float deltaKp, deltaKi, deltaKd;
    fuzzy_inference(fpid, error, de, &deltaKp, &deltaKi, &deltaKd);

    // 应用参数调整（带限幅）
    float new_Kp = fpid->Kp + deltaKp * fpid->deltaKp_scale;
    float new_Ki = fpid->Ki + deltaKi * fpid->deltaKi_scale;
    float new_Kd = fpid->Kd + deltaKd * fpid->deltaKd_scale;

    // 参数保护
    new_Kp = fmaxf(fmaxf(new_Kp, 0.1f), 5.0f);
    new_Ki = fmaxf(fmaxf(new_Ki, 0.0f), 2.0f);
    new_Kd = fmaxf(fmaxf(new_Kd, 0.0f), 1.0f);

    // 计算PID输出
    fpid->integral += new_Ki * error * dt;
    fpid->integral = fmaxf(fminf(fpid->integral, 100.0f), 0.0f); // 积分限幅

    float derivative = (measure - fpid->prev_measure) / dt;

    float output = new_Kp * error + fpid->integral - new_Kd * derivative;

    // 更新状态
    fpid->prev_error = error;
    fpid->prev_measure = measure;

    return fmaxf(fminf(output, 100.0f), 0.0f); // 输出限幅0-100%
}

// 示例测试
int main()
{
    FuzzyPIDController fpid = {
        .Kp = 1.0f, // 初始参数
        .Ki = 0.2f,
        .Kd = 0.1f};
    FuzzyPID_Init(&fpid);

    float setpoint = 100.0f;
    float measure = 20.0f; // 初始温度
    float dt = 0.1f;       // 100ms控制周期

    // 模拟温度系统（二阶惯性+延迟）
    for (int i = 0; i < 50; i++)
    {
        // 计算控制量
        float output = FuzzyPID_Compute(&fpid, setpoint, measure, dt);

        // 模拟被控对象（带噪声）
        measure += output * 0.1f * dt;
        measure += (rand() % 100 - 50) / 500.0f; // 添加±0.1°C噪声

        printf("T=%2d: SP=%.1f°C, PV=%.2f°C, Out=%.1f%%, Kp=%.2f, Ki=%.2f, Kd=%.2f\n",
               i + 1, setpoint, measure, output, fpid.Kp, fpid.Ki, fpid.Kd);

        // 模拟设定值变化
        if (i == 20)
            setpoint = 80.0f;
    }

    return 0;
}